
import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch_ros.actions import Node
from launch.substitutions import Command
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    
    # param(path,file content)
    gazebo_path = os.path.join(
        get_package_share_directory('gazebo_ros'),
        'launch'
    )
    world_path = ""  # world路径
    
    urdf_path = get_package_share_directory('robot_description')+'/urdf/arm_gazebo.xacro'
    
    robot_desc = Command([f'xacro {urdf_path}'])
    
    controller_file_path = os.path.join(
        get_package_share_directory('moveit_config'),
        'config/ros2_gazebo_controllers.yaml'
    )
    
    # node
    state_publisher = Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{
                'robot_description': robot_desc 
            }],
        )
    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                        arguments=['-entity', 'demo','-topic','/robot_description','-timeout','120'],
                        output='screen')

    arm_controller = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "arm_controller",
            "--controller-manager",
            "/controller_manager"
        ]
    )

    joint_state_broadcaster = Node(
            package="controller_manager",
            executable="spawner",
            arguments=[
                "joint_state_broadcaster",
                "--controller-manager",
                "/controller_manager"
            ]
        )

    # gazebo = IncludeLaunchDescription(PythonLaunchDescriptionSource([gazebo_path]))

    gazebo_server = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([gazebo_path, '/gzserver.launch.py']),
        launch_arguments=[
            # ('world',world_path), # 如果想要自定义世界就用这个
        ]
    )

    gazebo_client = IncludeLaunchDescription(PythonLaunchDescriptionSource([gazebo_path, '/gzclient.launch.py']))

    return LaunchDescription([
        gazebo_server,
        gazebo_client,
        state_publisher,
        spawn_entity,
        arm_controller,
        joint_state_broadcaster
    ])
    
    
